// 读取 txt中存储的文件

#include "ros/ros.h"
#include "std_msgs/Int8.h"
#include <fstream>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d.hpp>
#include <ros/package.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_datatypes.h>
struct initial_parameters
{
    std::string camera_topic;
    std::string radar_topic;
    std::pair<int,int> grid_size;
    int square_length; // in millimetres
    std::pair<int,int> board_dimension; // in millimetres
    cv::Mat cameramat;
    cv::Mat distcoeff;
}i_params;


int read_parameter()
{
    std::string pkg_loc;
    // Read input parameters from configuration file
    pkg_loc = ros::package::getPath("cr_optimizer");
    std::ifstream infile(pkg_loc + "/cfg/initial_params.txt");
    int cb_l, cb_b, l, b, e_l, e_b;
    infile >> i_params.camera_topic;
    infile >> i_params.radar_topic;

    infile >> cb_l;
    infile >> cb_b;
    i_params.grid_size = std::make_pair(cb_l, cb_b);

    infile >> i_params.square_length;

    infile >> l;
    infile >> b;
    i_params.board_dimension = std::make_pair(l, b);

    double camera_mat[9];
    for (int i = 0; i < 9; i++)
    {
        infile >> camera_mat[i];
    }
    cv::Mat(3, 3, CV_64F, &camera_mat).copyTo(i_params.cameramat);
    double dist_coeff[5];
    for (int i = 0; i < 5; i++)
    {
        infile >> dist_coeff[i];
    }
    cv::Mat(1, 5, CV_64F, &dist_coeff).copyTo(i_params.distcoeff);
}
int main(int argc, char **argv)
{
    ros::init(argc, argv, "input_sample");
    ros::NodeHandle sample("~");
    ROS_INFO("Just Optizimer NODE!");


    std::string filename = "text.txt";
    std::ifstream istrm(filename, std::ios::binary);
    std::vector<cv::Point3d> radar_points;
    std::vector<cv::Point2d> image_points;
    if (!istrm.is_open())
    {
        std::cout << "failed to open " << filename << '\n';
    }
    else
    {
        while(!istrm.eof() ){
            cv::Point2d image_point;
            cv::Point3d radar_point;
            double ix, iy, rx, ry, rz;
            istrm >> image_point.x;
            istrm >> image_point.y;
            istrm >> radar_point.x;
            istrm >> radar_point.y;
            istrm >> radar_point.z;
            std::cout << "read back from file: " << image_point.x << ' ' << image_point.y << ' ' 
                        << radar_point.x << ' ' << radar_point.y << ' ' << radar_point.z << '\n';
            radar_points.push_back(radar_point);
            image_points.push_back(image_point);
        }
        radar_points.pop_back();
        image_points.pop_back();

        ROS_INFO("Read Success");
        ROS_INFO("find %d points",radar_points.size());
        read_parameter();
        
    }

    if(radar_points.size() < 4)
    {
        ROS_ERROR("Insufficient feature points");
        return -1;
    }
    if (ros::ok())
    {
        cv::Mat rvec(3,3,cv::DataType<double>::type); // Initialization for pinhole and fisheye cameras
        cv::Mat tvec(3,1,cv::DataType<double>::type);
        //  pnp 求解
        cv::solvePnP(radar_points, image_points, i_params.cameramat, i_params.distcoeff, rvec, tvec);

        ROS_INFO("rotation");
        std::cout<< rvec<<std::endl;
        ROS_INFO("transtion");

        std::cout<< tvec<<std::endl;
        tf::Matrix3x3 rot;
        rot.setRPY(rvec.at<double>(0), rvec.at<double>(1), rvec.at<double>(2));
        cv::Mat tmp_rot = (cv::Mat_<double>(3, 3) << 
                            rot.getRow(0)[0], rot.getRow(0)[1], rot.getRow(0)[2],
                            rot.getRow(1)[0], rot.getRow(1)[1], rot.getRow(1)[2],
                            rot.getRow(2)[0], rot.getRow(2)[1], rot.getRow(2)[2]);
        std::cout<< tmp_rot<<std::endl;
        









        ros::spinOnce();
    }
    return 0;
}
